{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix\n",
    "import sympy\n",
    "import cv2\n",
    "from time import sleep\n",
    "from robot import r2t,t2r,tr2rpy,rpy2tr\n",
    "from robot import trotx,troty,trotz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao as mNao\n",
    "\n",
    "q0 = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "\n",
    "nao = mNao(name=\"nao\")\n",
    "\n",
    "nao.LeftHand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "tr 为末端位姿矩阵,m为掩模"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import cv2\n",
    "def get_tr(tr,th,p): # 获取变化后的值, 绕末端的 z 轴转动 th,沿着该轴移动 p\n",
    "    wz = tr*array([0,1,0,1]).reshape((4,1))\n",
    "    w = wz[:3,0]\n",
    "    w = w/np.linalg.norm(w)\n",
    "    R=cv2.Rodrigues(w*th)[0]\n",
    "    tr2 = tr.copy()\n",
    "    tr2[:3,:3] = tr[:3,:3]*R\n",
    "    tr2[:3,3] += p*w\n",
    "    return tr2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def get_delta(tr,tr2):\n",
    "    R = inv(tr[:3,:3])*tr2[:3,:3] # R 为绕末端坐标系的转动矩阵\n",
    "    w = cv2.Rodrigues(R)[0]\n",
    "    th = np.linalg.norm(w)\n",
    "    w = w/th\n",
    "    return w,th"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def ikfine2(robot,mask,tr,q0,verstep=True):\n",
    "    if isinstance(q0,list): q0 = np.array(q0)\n",
    "    if isinstance(tr,np.ndarray): tr = np.matrix(tr)\n",
    "    count = 0 # 迭代次数\n",
    "    ilimit = 10000 # 迭代次数\n",
    "    stol=1e-6 # 容忍误差\n",
    "    alpha= 1.0 # 步长\n",
    "    inf = np.inf\n",
    "    e = np.matrix([inf]*6) # 误差向量\n",
    "    e = e.transpose()\n",
    "    eprev = e.copy() # 前一个误差向量\n",
    "    save_q = None # 结果\n",
    "    nm = inf # 误差值\n",
    "    q = array(q0) # 迭代中的q,初始值为q0\n",
    "    save_q = q.copy()\n",
    "    save_e = e.copy()\n",
    "    def mynorm(e):\n",
    "        e2 = e.copy()\n",
    "        e2[0:3,0] *= 0.001\n",
    "        return np.linalg.norm(e[:,0])\n",
    "\n",
    "    while True:\n",
    "        count += 1\n",
    "        if count > ilimit: \n",
    "            print \"超出迭代限制\"\n",
    "            break\n",
    "        tq = robot.fkine(q)\n",
    "        e[0:3,0] = tr[:3,3] - tq[:3,3]\n",
    "        rq = t2r(tq)\n",
    "        e[3:6,0] = cv2.Rodrigues(rq.transpose()*t2r(tr))[0]\n",
    "        \n",
    "        if verstep and mynorm(e[mask,0]) < mynorm(save_e[mask,0]):\n",
    "            # 更好的e则扩大步长,变步长 方法\n",
    "            save_q = q.copy()\n",
    "            save_e = e.copy()\n",
    "            alpha *= 2**(1/8.0)\n",
    "        elif verstep:\n",
    "            # 使用保存的e,并缩短步长\n",
    "            q = save_q.copy()\n",
    "            e = save_e.copy()\n",
    "            alpha *= 0.5\n",
    "            \n",
    "        j = robot.jacob0(q)\n",
    "        #dq = j[mask,:].transpose()*e[mask,0]\n",
    "        dq = pinv(j[mask,:])*e[mask,0]\n",
    "        dq = alpha*dq\n",
    "        q += array(dq).flatten()\n",
    "        # wrap angles for revolute joints\n",
    "        k = q > pi\n",
    "        q[k] = q[k] - 2*pi\n",
    "        k = q < -pi\n",
    "        q[k] = q[k] + 2*pi\n",
    "        nm = mynorm(e[mask,0])\n",
    "        if mynorm(e[mask,0]) > mynorm(eprev[mask,0]) :\n",
    "            print \"警告,发散\"\n",
    "        eprev = e\n",
    "        if nm < stol:\n",
    "            print \"求解成功,q:\",q\n",
    "            print \"迭代次数:\",count\n",
    "            return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "robot = nao.LeftHand # 机器人\n",
    "m=(0,1,2,3,4) #自由度掩模,放弃绕0坐标系的z方向转动的自由度,即绕末端的y轴转动\n",
    "q0 = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "tr = robot.fkine(q0) # 末端矩阵\n",
    "tr = get_tr(tr,0.4,10) # 绕末端轴转 0.4, 平移 10mm后的末端位姿,为要求逆解的末端矩阵\n",
    "q = ikfine2(robot,m,tr,q0)\n",
    "Matrix(tr - robot.fkine(q))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q0 = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "robot = nao.LeftHand\n",
    "mask = (0,1,2,3,4) # 放弃绕0z轴转动\n",
    "tr = nao.LeftHand.fkine(q0)\n",
    "tr[0,3] += 10 # x 方向增加 10mm\n",
    "tr[1,3] += 5 # y 方向增加 5mm\n",
    "q = ikfine2(robot,mask,tr,q0)\n",
    "print get_delta(tr,robot.fkine(q))\n",
    "Matrix(tr-robot.fkine(q))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "检验模块中的代码"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.testing.assert_almost_equal(q,robot.ikine2(tr,q0),5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 绕定轴转动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 特别定制的数值逆解\n",
    "def ikfine2(robot,mask,tr,q0,verstep=True):\n",
    "    if isinstance(q0,list): q0 = np.array(q0)\n",
    "    if isinstance(tr,np.ndarray): tr = np.matrix(tr)\n",
    "    count = 0 # 迭代次数\n",
    "    ilimit = 1000 # 迭代次数\n",
    "    stol=1e-6 # 容忍误差\n",
    "    alpha= 1.0 # 步长\n",
    "    inf = np.inf\n",
    "    e = np.matrix([inf]*6) # 误差向量\n",
    "    e = e.transpose()\n",
    "    eprev = e.copy() # 前一个误差向量\n",
    "    save_q = None # 结果\n",
    "    nm = inf # 误差值\n",
    "    q = array(q0) # 迭代中的q,初始值为q0\n",
    "    save_q = q.copy()\n",
    "    save_e = e.copy()\n",
    "    invtr = inv(tr) # 向末端执行器变换的矩阵\n",
    "    tr = np.matrix(np.eye(4))\n",
    "    def mynorm(e):\n",
    "        e2 = e.copy()\n",
    "        e2[0:3,0] *= 0.001\n",
    "        return np.linalg.norm(e[:,0])\n",
    "\n",
    "    while True:\n",
    "        count += 1\n",
    "        if count > ilimit: \n",
    "            print \"超出迭代限制\"\n",
    "            print \"迭代次数:\",count\n",
    "            print \"误差:\",nm\n",
    "            return q,nm\n",
    "            break\n",
    "        tq = robot.fkine(q)\n",
    "        tq = invtr*tq # 在目标末端坐标系下的坐标矩阵\n",
    "        e[0:3,0] = - tq[:3,3]\n",
    "        rq = t2r(tq)\n",
    "        e[3:6,0] = cv2.Rodrigues(rq.transpose())[0]\n",
    "        \n",
    "        if verstep and mynorm(e[mask,0]) < mynorm(save_e[mask,0]):\n",
    "            # 更好的e则扩大步长,变步长 方法\n",
    "            save_q = q.copy()\n",
    "            save_e = e.copy()\n",
    "            alpha *= 2**(1/8.0)\n",
    "        elif verstep:\n",
    "            # 使用保存的e,并缩短步长\n",
    "            q = save_q.copy()\n",
    "            e = save_e.copy()\n",
    "            alpha *= 0.5\n",
    "            \n",
    "        j = robot.jacobn(q)\n",
    "        #dq = j[mask,:].transpose()*e[mask,0]\n",
    "        dq = pinv(j[mask,:])*e[mask,0]\n",
    "        dq = alpha*dq\n",
    "        q += array(dq).flatten()\n",
    "        # wrap angles for revolute joints\n",
    "        k = q > pi\n",
    "        q[k] = q[k] - 2*pi\n",
    "        k = q < -pi\n",
    "        q[k] = q[k] + 2*pi\n",
    "        nm = mynorm(e[mask,0])\n",
    "        if mynorm(e[mask,0]) > mynorm(eprev[mask,0]) :\n",
    "            print \"警告,发散\"\n",
    "        eprev = e\n",
    "        if nm < stol:\n",
    "            print \"求解成功,q:\",q\n",
    "            print \"迭代次数:\",count\n",
    "            return q,0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q0 = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "robot = nao.LeftHand\n",
    "mask = (0,1,2,3,5) # 放弃绕末端y轴转动\n",
    "tr = nao.LeftHand.fkine(q0)\n",
    "tr[0,3] += -10 # x 方向增加 -10mm\n",
    "tr[1,3] += 10 # y 方向增加 10mm\n",
    "tr[2,3] += 10 # z 方向增加 10mm\n",
    "q,err = ikfine2(robot,mask,tr,q0)\n",
    "print get_delta(tr,robot.fkine(q))\n",
    "Matrix(tr-robot.fkine(q))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr*np.matrix([0,1,0,1]).transpose() - robot.fkine(q)*np.matrix([0,1,0,1]).transpose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## webots 检验"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from nao import Nao as wNao\n",
    "wnao = wNao(\"127.0.0.1\",9559)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "wnao.simulationInitPose()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr = wnao.getLArmTf()\n",
    "tr.r2_c4 += 0.01 # y 方向移动 1cm\n",
    "tr = wnao.tf2nparray(tr)\n",
    "tr[:3,3] *= 1000 # 单位换算\n",
    "tr = np.matrix(tr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q0 = wnao.getLArmAngles()\n",
    "q0 = q0[:5]\n",
    "m=(0,1,2,3,5) # 放弃绕末端y轴的转动,即手握紧的轴\n",
    "q,err = ikfine2(nao.LeftHand,m,tr, q0)\n",
    "Matrix(tr - nao.LeftHand.fkine(q))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "if isinstance(q,np.ndarray): q=q.tolist()\n",
    "if isinstance(q0,np.ndarray): q0=q0.tolist()\n",
    "for x in range(10):\n",
    "    wnao.setLArmAngles(q+[0.0])\n",
    "    sleep(0.5)\n",
    "    wnao.setLArmAngles(q0+[0.0])\n",
    "    sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
